#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Bool
from geometry_msgs.msg import PoseStamped, TransformStamped
from moveit_msgs.msg import MoveItErrorCodes
from moveit_msgs.srv import GetPositionIK
from moveit_commander import MoveGroupCommander, RobotCommander, PlanningSceneInterface
import numpy as np
from scipy.spatial.transform import Rotation
from tf2_ros import TransformListener, Buffer, TransformBroadcaster
from tf2_geometry_msgs import do_transform_pose
from tf_transformations import quaternion_multiply, quaternion_inverse, quaternion_matrix, quaternion_from_matrix
import copy

############################# 全局变量 ###########################
obj_pose_msg = None
button_pose_msg = None
machine_cup_pose_msg = None
handover_pose_msg = None

tool_quat_compensate = None

class CoffeeTaskManager(Node):

    def __init__(self):
        super().__init__('coffee_task_manager')
        self.task_flag = 'wait_for_command'
        self.state_flag = 'stand_by'
        self.coffee_state = 0  # 0空 1非空

        # TF2 相关
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.tf_broadcaster = TransformBroadcaster(self)

        # MoveIt 初始化
        self.move_group_right_arm = MoveGroupCommander("right_arm", wait_for_servers=10.0)
        self.move_group_right_arm.set_end_effector_link("right_tool_link")
        self.move_group_right_arm.set_planning_time(0.5)
        self.move_group_right_arm.set_max_velocity_scaling_factor(0.2)
        self.move_group_right_arm.set_max_acceleration_scaling_factor(0.2)

        self.move_group_left_arm = MoveGroupCommander("left_arm", wait_for_servers=10.0)
        self.move_group_left_arm.set_end_effector_link("Left_Index_Finger_Link3")
        self.move_group_left_arm.set_planning_time(0.5)
        self.move_group_left_arm.set_max_velocity_scaling_factor(0.3)
        self.move_group_left_arm.set_max_acceleration_scaling_factor(0.3)

        # 发布者和订阅者
        self.hand_control_publisher = self.create_publisher(Bool, "/hand_control_topic", 10)
        self.obj_pose_subscriber = self.create_subscription(PoseStamped, "/obj_pose", self.obj_pose_callback, 10)
        self.button_pose_subscriber = self.create_subscription(PoseStamped, "/button_pose", self.button_pose_callback, 10)
        self.machine_cup_pose_subscriber = self.create_subscription(PoseStamped, "/machine_cup_pose", self.machine_cup_pose_callback, 10)
        self.handover_pose_subscriber = self.create_subscription(PoseStamped, "/handover_pose", self.handover_pose_callback, 10)
        self.upper_body_command_subscriber = self.create_subscription(String, "/upper_body_command", self.upper_body_callback, 10)

    def state_refresh(self, new_state):
        if new_state == 'grasp_stand_by':
            if self.coffee_state == 0:
                self.task_flag = "put_on_table"
            else:
                self.task_flag = "handover"
        elif new_state == 'put_finger_arrive':
            if self.coffee_state == 0:
                self.coffee_state = 1  #################### 暂时先没加左手按咖啡机
            elif self.coffee_state == 1:
                self.task_flag = 'finished'  ############# 放置且是满杯 则转为完成状态
        elif new_state == 'stand_by':
            self.task_flag = 'wait_for_command'
        self.get_logger().info(f"Coffee_Task_Manager`s state_flag was {self.state_flag} ")
        self.state_flag = new_state
        self.get_logger().info(f"Coffee_Task_Manager`s state_flag is {self.state_flag} now !!!")

    def obj_pose_callback(self, msg):
        global obj_pose_msg
        obj_pose_msg = msg

        P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
        quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y,
                             obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]
        R_object = quaternion_matrix(quaternion_object)[:3, :3]
        self.tf_pub("base_link", "obj_link", P_object, R_object)

    def tf_pub(self, parent_frame, child_frame, translation, rotation_matrix):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = parent_frame
        t.child_frame_id = child_frame
        t.transform.translation.x = translation[0]
        t.transform.translation.y = translation[1]
        t.transform.translation.z = translation[2]
        quat = quaternion_from_matrix(rotation_matrix)
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]
        self.tf_broadcaster.sendTransform(t)

    def button_pose_callback(self, msg):
        global button_pose_msg
        if msg.pose.orientation.x != -1 and msg.pose.orientation.y != -1:
            button_pose_msg = msg
        P_machine = np.array(
            [button_pose_msg.pose.position.x, button_pose_msg.pose.position.y, button_pose_msg.pose.position.z])
        quaternion_machine = [button_pose_msg.pose.orientation.x, button_pose_msg.pose.orientation.y,
                              button_pose_msg.pose.orientation.z, button_pose_msg.pose.orientation.w]
        R_machine = quaternion_matrix(quaternion_machine)[:3, :3]
        self.tf_pub("base_link", "button_link", P_machine, R_machine)

    def machine_cup_pose_callback(self, msg):
        global machine_cup_pose_msg
        if msg.pose.orientation.x != -1 and msg.pose.orientation.y != -1:
            machine_cup_pose_msg = msg
        P_machine = np.array([machine_cup_pose_msg.pose.position.x, machine_cup_pose_msg.pose.position.y,
                              machine_cup_pose_msg.pose.position.z])
        quaternion_machine = [machine_cup_pose_msg.pose.orientation.x, machine_cup_pose_msg.pose.orientation.y,
                              machine_cup_pose_msg.pose.orientation.z, machine_cup_pose_msg.pose.orientation.w]
        R_machine = quaternion_matrix(quaternion_machine)[:3, :3]
        self.tf_pub("base_link", "machine_cup_link", P_machine, R_machine)

    def handover_pose_callback(self, msg):
        global handover_pose_msg
        if msg.pose.orientation.x != -1 and msg.pose.orientation.y != -1:
            handover_pose_msg = msg
        P_handover = np.array(
            [handover_pose_msg.pose.position.x, handover_pose_msg.pose.position.y, handover_pose_msg.pose.position.z])
        quaternion_handover = [handover_pose_msg.pose.orientation.x, handover_pose_msg.pose.orientation.y,
                               handover_pose_msg.pose.orientation.z, handover_pose_msg.pose.orientation.w]
        R_handover = quaternion_matrix(quaternion_handover)[:3, :3]
        self.tf_pub("base_link", "handover_link", P_handover, R_handover)

    def upper_body_callback(self, msg):
        if self.task_flag == "wait_for_command":
            self.move_to_stand_by_pose()
            self.task_flag = msg.data
            self.task_run()
        else:
            self.get_logger().info(f"Failed to set task flag as task_flag is {self.task_flag} now !!!")

    def left_arm_move_to_pos(self, position, link_name):
        current_pose = self.move_group_left_arm.get_current_pose().pose

        move_pose_msg = Pose()
        move_pose_msg.position.x = position[0]
        move_pose_msg.position.y = position[1]
        move_pose_msg.position.z = position[2]
        move_pose_msg.orientation.x = -0.10711095938856535
        move_pose_msg.orientation.y = -0.33227448354515043
        move_pose_msg.orientation.z = -0.845326815249489
        move_pose_msg.orientation.w = 0.40440510059075774

        self.get_logger().info(f"adjusted_right_pose: {move_pose_msg}")
        self.get_logger().info(f"current_pose: {current_pose}")

        step_size = 0.01
        num_steps = 4

        for i in range(num_steps):
            interpolation_factor = (i + 1) / num_steps
            intermediate_pose = Pose()
            intermediate_pose.position.x = current_pose.position.x + (position[0] - current_pose.position.x) * interpolation_factor
            intermediate_pose.position.y = current_pose.position.y + (position[1] - current_pose.position.y) * interpolation_factor
            intermediate_pose.position.z = current_pose.position.z + (position[2] - current_pose.position.z) * interpolation_factor

            intermediate_pose.orientation.x = -0.10711095938856535
            intermediate_pose.orientation.y = -0.33227448354515043
            intermediate_pose.orientation.z = -0.845326815249489
            intermediate_pose.orientation.w = 0.40440510059075774

            self.move_group_left_arm.set_pose_target(intermediate_pose)
            plan = self.move_group_left_arm.plan()

            if plan[0]:
                success = self.move_group_left_arm.execute(plan[1], wait=True)
                self.move_group_left_arm.stop()
                self.move_group_left_arm.clear_pose_targets()
                if not success:
                    self.get_logger().warn(f"Failed to move {link_name} to intermediate pose at step {i + 1}.")
                    return
            else:
                self.move_group_left_arm.stop()
                self.move_group_left_arm.clear_pose_targets()
                self.get_logger().warn(f"Failed to generate plan for {link_name} at step {i + 1}.")
                return

        self.get_logger().info(f"Successfully moved {link_name} to the target pose in {num_steps} steps.")

    def press_button_callback(self):
        button_pose = copy.deepcopy(button_pose_msg)

        button_pose_quaternion = [button_pose.pose.orientation.x, button_pose.pose.orientation.y,
                                  button_pose.pose.orientation.z, button_pose.pose.orientation.w]
        button_pose_rotation_matrix = quaternion_matrix(button_pose_quaternion)

        local_offset_far = np.array([-0.01, 0.01, 0.10, 1.0])
        local_offset_near = np.array([-0.01, 0.01, 0.00, 1.0])
        world_offset_far = np.dot(button_pose_rotation_matrix, local_offset_far)
        world_offset_near = np.dot(button_pose_rotation_matrix, local_offset_near)

        pose_far = [-1, -1, -1]
        pose_far[0] = button_pose.pose.position.x + world_offset_far[0]
        pose_far[1] = button_pose.pose.position.y + world_offset_far[1]
        pose_far[2] = button_pose.pose.position.z + world_offset_far[2]

        self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.2))

        pose_near = [-1, -1, -1]
        pose_near[0] = button_pose.pose.position.x + world_offset_near[0]
        pose_near[1] = button_pose.pose.position.y + world_offset_near[1]
        pose_near[2] = button_pose.pose.position.z + world_offset_near[2]
        self.left_arm_move_to_pos(pose_near, "Left_arm_Link")

        self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.2))

        pose_far = [-1, -1, -1]
        pose_far[0] = button_pose.pose.position.x + world_offset_far[0]
        pose_far[1] = button_pose.pose.position.y + world_offset_far[1]
        pose_far[2] = button_pose.pose.position.z + world_offset_far[2]
        self.left_arm_move_to_pos(pose_far, "Left_arm_Link")

    def compute_hand_position(self, P_object, R_object, dimensions, hand_y_toward):
        r, h = dimensions
        obj_x_dir = R_object[:, 0]
        obj_y_dir = R_object[:, 1]
        obj_z_dir = R_object[:, 2]

        tool_x_dir = -obj_z_dir
        tool_x_dir = tool_x_dir / np.linalg.norm(tool_x_dir)

        if hand_y_toward == [0, -1, 0]:
            tool_y_candidate = np.array([0.0, -1.0, 0.0])
            dot_xy = np.abs(np.dot(tool_x_dir, tool_y_candidate))
            if dot_xy > 0.99:
                tool_y_candidate = np.array([0.0, 0.0, -1.0])
            v_perp = tool_y_candidate - np.dot(tool_y_candidate, tool_x_dir) * tool_x_dir
            norm_vp = np.linalg.norm(v_perp)
            if norm_vp < 1e-8:
                tool_y_candidate = np.array([0.0, 0.0, 1.0])
                v_perp = tool_y_candidate - np.dot(tool_y_candidate, tool_x_dir) * tool_x_dir
                norm_vp = np.linalg.norm(v_perp)
        else:
            tool_y_candidate = hand_y_toward / np.linalg.norm(hand_y_toward)
            dot_xy = np.abs(np.dot(tool_x_dir, tool_y_candidate))
            if dot_xy > 0.99:
                return False, None, None
            v_perp = tool_y_candidate - np.dot(tool_y_candidate, tool_x_dir) * tool_x_dir
            norm_vp = np.linalg.norm(v_perp)

        tool_y_dir = v_perp / norm_vp
        tool_z_dir = np.cross(tool_x_dir, tool_y_dir)
        tool_z_dir = tool_z_dir / np.linalg.norm(tool_z_dir)

        grasp_orientation = np.column_stack((tool_x_dir, tool_y_dir, tool_z_dir))
        grasp_poses = []
        grasp_poses.append({
            "position": P_object + grasp_orientation @ np.array([0.03, r, 0.068]),
            "orientation": grasp_orientation
        })
        self.tf_pub("base_link", "target_tool_link", grasp_poses[0]["position"], grasp_poses[0]["orientation"])

        return True, grasp_poses[0]["position"], grasp_poses[0]["orientation"]

    def compute_new_compensation(self, current_pos, current_quat, target_pos, target_quat):
        current_quat_inv = quaternion_inverse(current_quat)
        delta_quat = quaternion_multiply(target_quat, current_quat_inv)
        delta_pos = target_pos - current_pos
        return delta_pos, delta_quat

    def move_to_pose(self, position, quaternion, link_name, use_oc=False):
        move_pose_msg = Pose()
        move_pose_msg.position.x = position[0]
        move_pose_msg.position.y = position[1]
        move_pose_msg.position.z = position[2]
        move_pose_msg.orientation.x = quaternion[0]
        move_pose_msg.orientation.y = quaternion[1]
        move_pose_msg.orientation.z = quaternion[2]
        move_pose_msg.orientation.w = quaternion[3]

        self.get_logger().info(f"adjusted_right_pose: {move_pose_msg}")
        self.get_logger().info(f"current_pose: {self.move_group_right_arm.get_current_pose().pose}")

        self.move_group_right_arm.set_pose_target(move_pose_msg)
        plan = self.move_group_right_arm.plan()

        res_execute = self.move_group_right_arm.execute(plan[1], wait=True)
        self.move_group_right_arm.stop()
        self.move_group_right_arm.clear_pose_targets()
        self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.2))

        if plan[0] and res_execute:
            self.get_logger().info(f"Successfully moved {link_name} to the target pose.")
        else:
            self.get_logger().warn(f"Failed to move {link_name} to the target pose.")
        return plan[0] and res_execute

    def is_pose_similar(self, position1, quaternion1, position2, quaternion2, max_translation_error=0.01, max_rotation_error=10.0):
        position1 = np.array(position1)
        position2 = np.array(position2)
        translation_error = np.linalg.norm(position1 - position2)

        quat1 = Rotation.from_quat(quaternion1)
        quat2 = Rotation.from_quat(quaternion2)
        relative_rotation = quat1.inv() * quat2
        rotation_error = relative_rotation.magnitude() * (180 / np.pi)

        self.get_logger().info(f"translation_error: {translation_error}")
        self.get_logger().info(f"rotation_error: {rotation_error}")

        return translation_error <= max_translation_error and rotation_error <= max_rotation_error

    def quat_multiply(self, quat1, quat2):
        qx1, qy1, qz1, qw1 = quat1
        qx2, qy2, qz2, qw2 = quat2

        qx = qw1 * qx2 + qx1 * qw2 - qy1 * qz2 + qz1 * qy2
        qy = qw1 * qy2 + qy1 * qw2 + qx1 * qz2 - qz1 * qx2
        qz = qw1 * qz2 - qx1 * qy2 + qy1 * qx2 + qz1 * qw2
        qw = qw1 * qw2 - qx1 * qx2 - qy1 * qy2 - qz1 * qz2

        return [qx, qy, qz, qw]

    def move_to_stand_by_pose(self, is_use_compensate=False):
        stand_by_position = [0.705, -0.367, 0.951 + 0.1]
        stand_by_quaternion = [0.551, -0.388, 0.621, 0.400]

        if is_use_compensate:
            stand_by_quaternion = self.quat_multiply(stand_by_quaternion, tool_quat_compensate)

        res_execute = self.move_to_pose(stand_by_position, stand_by_quaternion, "Right_Arm_Link7")

    def calculate_grasp_pose(self, P_object, R_object, dimensions, grasp_y_toward):
        res_cacu_grasp, P_grasp, grasp_orientation = self.compute_hand_position(P_object, R_object, dimensions, hand_y_toward=grasp_y_toward)
        if not res_cacu_grasp:
            self.get_logger().info(f"Failed to calculate grasp Pose")
        return res_cacu_grasp, P_grasp, grasp_orientation

    def compute_world_offset(self, grasp_quaternion, pre_grasp_pose):
        if pre_grasp_pose == 'beyond':
            local_offset = np.array([0.15, 0.0, 0.0, 1.0])
        elif pre_grasp_pose == 'front':
            local_offset = np.array([0.0, 0.1, 0.0, 1.0])
        else:
            self.get_logger().warn(f"Unknown pre_grasp_pose: {pre_grasp_pose}")
            return np.zeros(4)

        return np.dot(quaternion_matrix(grasp_quaternion), local_offset)

    def grasp_task(self, grasp_y_toward, pre_grasp_pose):
        global coffee_task_manager
        falied_times = 0
        grasp_y_toward_copy = copy.deepcopy(grasp_y_toward)

        while coffee_task_manager.state_flag != 'grasp_hand_arrive':
            P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
            quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y,
                                 obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

            R_object = quaternion_matrix(quaternion_object)[:3, :3]

            res_cacu_grasp, P_grasp, grasp_orientation = self.calculate_grasp_pose(P_object, R_object, dimensions, grasp_y_toward)

            if not res_cacu_grasp:
                falied_times += 1
                if falied_times >= 3:
                    self.move_to_stand_by_pose()
                    coffee_task_manager.state_refresh('stand_by')
                    return False
                grasp_y_toward[0] *= 0.5
                continue

            world_offset = self.compute_world_offset(grasp_orientation, pre_grasp_pose)

            P_grasp_offset = P_grasp + world_offset[:3]

            res_execute = self.move_to_pose(P_grasp_offset, quaternion_from_matrix(np.eye(4)[:3, :3]), "Right_Arm_Link7", use_oc=True)
            if not res_execute:
                falied_times += 1
                if falied_times >= 3:
                    self.move_to_stand_by_pose()
                    coffee_task_manager.state_refresh('stand_by')
                    return False
                continue

            falied_times = 0
            self.get_logger().info(f"Successfully moved to pre-grasp position")

            new_P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
            new_quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y,
                                     obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]
            new_R_object = quaternion_matrix(new_quaternion_object)[:3, :3]

            res_cacu_grasp, new_P_grasp, new_grasp_orientation = self.calculate_grasp_pose(new_P_object, new_R_object, dimensions, grasp_y_toward)

            res_similar = self.is_pose_similar(P_grasp, grasp_orientation, new_P_grasp, new_grasp_orientation, max_translation_error=0.05, max_rotation_error=30.0)
            if res_similar:
                coffee_task_manager.state_refresh('grasp_arm_arrive')
            else:
                continue

            res_execute = self.move_to_pose(new_P_grasp, quaternion_from_matrix(np.eye(4)[:3, :3]), "Right_Arm_Link7")
            if not res_execute:
                self.move_to_stand_by_pose()
                coffee_task_manager.state_refresh('stand_by')
                return False

            coffee_task_manager.state_refresh('grasp_hand_arrive')

        msg_hand_grasp = Bool(data=False)
        self.hand_control_publisher.publish(msg_hand_grasp)
        self.get_logger().info(f"Publish False at {self.get_clock().now().to_msg()}")
        self.get_clock().sleep_for(rclpy.duration.Duration(seconds=2))

        ag_P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
        ag_quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y,
                                obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

        ag_R_object = quaternion_matrix(ag_quaternion_object)[:3, :3]
        res_cacu_grasp, ag_P_grasp, ag_grasp_orientation = self.calculate_grasp_pose(ag_P_object, ag_R_object, dimensions, grasp_y_toward)

        res_similar = self.is_pose_similar(new_P_grasp, new_grasp_orientation, ag_P_grasp, ag_grasp_orientation, max_translation_error=0.05, max_rotation_error=50.0)
        if not res_similar:
            msg_hand_grasp.data = True
            self.hand_control_publisher.publish(msg_hand_grasp)
            self.get_logger().info(f"Publish True at {self.get_clock().now().to_msg()}")
            self.get_clock().sleep_for(rclpy.duration.Duration(seconds=2))
            self.move_to_stand_by_pose()
            coffee_task_manager.state_refresh('stand_by')
            return False

        tool_pos_compensate, tool_quat_compensate = self.compute_new_compensation(ag_P_grasp, ag_grasp_orientation, new_P_grasp, new_grasp_orientation)

        self.move_to_stand_by_pose(is_use_compensate=True)
        coffee_task_manager.state_refresh('grasp_stand_by')

        return True

    def update_pose_and_orientation(self, P_object, quaternion_object, dimensions, hand_y_toward):
        R_object = quaternion_matrix(quaternion_object)[:3, :3]
        res_cacu, P_release, release_orientation = self.compute_hand_position(P_object, R_object, dimensions, hand_y_toward)

        release_orientation_homogeneous = np.eye(4)
        release_orientation_homogeneous[:3, :3] = release_orientation
        release_quaternion = quaternion_from_matrix(release_orientation_homogeneous)

        return P_release, release_quaternion, release_orientation

    def move_to_pose_with_retry(self, P_target, quaternion_target, retries=3):
        failed_times = 0
        while failed_times < retries:
            res_execute = self.move_to_pose(P_target, quaternion_target, "Right_Arm_Link7")
            if res_execute:
                return True
            failed_times += 1
        return False

    def compensate_pose(self, P_release, release_quaternion, tool_pos_compensate, tool_quat_compensate):
        world_offset = np.dot(quaternion_matrix(release_quaternion), np.array([0.0, 0.1, 0.0, 1.0]))
        P_pre_release = P_release + world_offset[:3] + tool_pos_compensate
        pre_release_quaternion = self.quat_multiply(release_quaternion, tool_quat_compensate)

        return P_pre_release, pre_release_quaternion

    def task_run(self):
        global coffee_task_manager

        shape = 'cylinder'
        dimensions = [0.055, 0.106]

        coffee_task_manager.state_refresh('grasp_arm_go')

        res_grasp = self.grasp_task(grasp_y_toward=[-1, -1, 0], pre_grasp_pose='beyond')
        if not res_grasp:
            return

        falied_times = 0
        while not coffee_task_manager.state_flag == 'put_finger_arrive':
            P_machine_cup = np.array([machine_cup_pose_msg.pose.position.x, machine_cup_pose_msg.pose.position.y,
                                      machine_cup_pose_msg.pose.position.z])
            quaternion_machine_cup = [machine_cup_pose_msg.pose.orientation.x, machine_cup_pose_msg.pose.orientation.y,
                                      machine_cup_pose_msg.pose.orientation.z, machine_cup_pose_msg.pose.orientation.w]

            P_release, release_quaternion, release_orientation = self.update_pose_and_orientation(P_machine_cup,
                                                                                                 quaternion_machine_cup,
                                                                                                 dimensions,
                                                                                                 hand_y_toward=[-1, -1, 0])

            P_pre_release, pre_release_quaternion = self.compensate_pose(P_release, release_quaternion, tool_pos_compensate,
                                                                        tool_quat_compensate)

            self.tf_pub("base_link", "pre_release", P_pre_release, pre_release_quaternion, is_quat=True)

            if not self.move_to_pose_with_retry(P_pre_release, pre_release_quaternion):
                msg_hand_release = Bool()
                msg_hand_release.data = True
                self.hand_control_publisher.publish(msg_hand_release)
                self.get_clock().sleep_for(rclpy.duration.Duration(seconds=2))
                return

            br_P_object = np.array([obj_pose_msg.pose.position.x, obj_pose_msg.pose.position.y, obj_pose_msg.pose.position.z])
            br_quaternion_object = [obj_pose_msg.pose.orientation.x, obj_pose_msg.pose.orientation.y,
                                    obj_pose_msg.pose.orientation.z, obj_pose_msg.pose.orientation.w]

            br_P_release, br_release_quaternion, br_release_orientation = self.update_pose_and_orientation(br_P_object,
                                                                                                          br_quaternion_object,
                                                                                                          dimensions,
                                                                                                          hand_y_toward=[-1, -1, 0])

            res_similar = self.is_pose_similar(P_pre_release, pre_release_quaternion, br_P_release, br_release_quaternion,
                                              max_translation_error=0.08, max_rotation_error=50.0)
            if not res_similar:
                msg_hand_grasp = Bool()
                msg_hand_grasp.data = True
                self.hand_control_publisher.publish(msg_hand_grasp)
                self.get_clock().sleep_for(rclpy.duration.Duration(seconds=2))
                return False

            tool_pos_compensate_inc, tool_quat_compensate_inc = self.compute_new_compensation(br_P_object, br_release_quaternion,
                                                                                             P_release, release_quaternion)
            tool_quat_compensate = self.quat_multiply(tool_quat_compensate, tool_quat_compensate_inc)
            tool_pos_compensate += tool_pos_compensate_inc

            release_quaternion_compensate = self.quat_multiply(release_quaternion, tool_quat_compensate)
            P_release_compensate = P_release + tool_pos_compensate
            self.tf_pub("base_link", "P_release_compensate", P_release_compensate, release_quaternion_compensate, is_quat=True)

            P_release[0] += 0.023
            P_release[1] -= 0.01
            if not self.move_to_pose_with_retry(P_release, release_quaternion_compensate):
                return

            coffee_task_manager.state_refresh('put_arm_arrive')

            msg_hand_release = Bool()
            msg_hand_release.data = True
            self.hand_control_publisher.publish(msg_hand_release)
            self.get_clock().sleep_for(rclpy.duration.Duration(seconds=2))

            coffee_task_manager.state_refresh('put_finger_arrive')

        self.press_button_callback()
        self.get_clock().sleep_for(rclpy.duration.Duration(seconds=5))

        self.move_group_right_arm.set_max_velocity_scaling_factor(0.3)
        self.move_group_right_arm.set_max_acceleration_scaling_factor(0.3)

        res_execute = self.move_to_pose(P_pre_release, pre_release_quaternion, "Right_Arm_Link7")
        self.move_to_stand_by_pose(is_use_compensate=True)
        coffee_task_manager.state_refresh('grasp_stand_by')

        while not coffee_task_manager.state_flag == 'put_finger_arrive':
            P_handover = np.array([handover_pose_msg.pose.position.x, handover_pose_msg.pose.position.y, handover_pose_msg.pose.position.z])
            handover_quaternion = [handover_pose_msg.pose.orientation.x, handover_pose_msg.pose.orientation.y,
                                   handover_pose_msg.pose.orientation.z, handover_pose_msg.pose.orientation.w]

            handover_quaternion_compensate = self.quat_multiply(handover_quaternion, tool_quat_compensate)
            P_handover_compensate = P_handover + tool_pos_compensate

            if not self.move_to_pose_with_retry(P_handover_compensate, handover_quaternion_compensate):
                return

            coffee_task_manager.state_refresh('put_arm_arrive')

            msg_hand_release = Bool()
            msg_hand_release.data = True
            self.hand_control_publisher.publish(msg_hand_release)
            self.get_clock().sleep_for(rclpy.duration.Duration(seconds=2))

            coffee_task_manager.state_refresh('put_finger_arrive')

        self.move_to_stand_by_pose()
        coffee_task_manager.state_refresh('stand_by')


def main(args=None):
    rclpy.init(args=args)
    coffee_task_manager = CoffeeTaskManager()
    rclpy.spin(coffee_task_manager)
    coffee_task_manager.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()